#include <custom_msg/GPS.h>
#include <ros/ros.h>
#include <serial/serial.h>  //ROS已经内置了的串口�?#include <std_msgs/Empty.h>
#include <std_msgs/String.h>

#include <algorithm>  //replace method
#include <cmath>
#include <cstdlib>  //string转化为double
#include <iomanip>  //保留有效小数
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include "commons/rabbit_consts.h"
using namespace std;

#define min(a,b)                      (((a) < (b)) ? (a) : (b))
serial::Serial ser;        //声明串口对象
custom_msg::GPS GPS_data;  //全局变量，解析后数据

int main11(int argc, char** argv) {
  int len, Rece_out;
  ros::init(argc, argv, "serial_node_gps");
  ros::NodeHandle nh;
  ros::Publisher GPS_pub = nh.advertise<custom_msg::GPS>(TOPIC_GPS, 1000);
  //设置循环的频�?50HZ 20ms 要求循环频率大于数据接收频率
  ros::Rate loop_rate(50);
  string strRece, str[13];
  int i = 0, start = -1, end = -1;
  while (true) {
    GPS_data.header.stamp = ros::Time::now(); // 需要为 gps 添加 header
    GPS_data.lat = 22.3;
    GPS_data.N_S = "N";
    GPS_data.lon = 22.3;
    GPS_data.E_W = "W";
    GPS_data.groundSpeed = 22.3;
    GPS_data.speedDir = 22.3;
    GPS_data.gpsAngle = 22.3;
    GPS_data.gpsAngleDir = "N";
    GPS_pub.publish(GPS_data);
    ros::spinOnce();
    loop_rate.sleep();
  }
}

int main(int argc, char** argv) {
  int len, Rece_out;
  ros::init(argc, argv, "serial_node_gps");
  ros::NodeHandle nh;
  ros::Publisher GPS_pub = nh.advertise<custom_msg::GPS>(TOPIC_GPS, 1000);
  try {
    ser.setPort(SERIAL_GPS_DEV);
    ser.setBaudrate(115200);
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ser.setTimeout(to);
    ser.open();
  } catch (serial::IOException& e) {
    ROS_ERROR_STREAM("Unable to open Serial Port !");
    return -1;
  }
  if (ser.isOpen()) {
    ROS_INFO_STREAM("Serial Port initialized");
  } else {
    return -1;
  }
  //设置循环的频�?50HZ 20ms 要求循环频率大于数据接收频率
  ros::Rate loop_rate(500);
  string strRece, str[13];
  size_t position;
  int i = 0, start = -1, end = -1, pos = -1;
  while (ros::ok()) {
    len = ser.available();
    if (len > 0) {
      strRece += ser.read(len);
      //strRece="$GNRMC,080837.40,A,3016.6169638,N,12000.5391099,E,2.54,360.11,150921,0.0,E,A*29\r\n";
      while (i < strRece.length()) {
        start = strRece.find(SERIAL_GPS_START);
        if (start == -1)  // not find head
        {
          if (strRece.length() > 2) {
            strRece = strRece.substr(strRece.length() - 3);
          }
          break;
        } else {
          end = strRece.find(SERIAL_RANGE_END);
          if (end == -1) {
            if (start != 0) {
              strRece = strRece.substr(start);
            }
            break;
          } else {
            i = end;
             cout << "all gps:" << strRece<< "\r\n";
            strRece.substr(start, end + 2 - start);
            pos= strRece.find("RMC");
            if (pos != -1) {
              if (strRece.size()<30){
                break;
              }
              replace(strRece.begin(), strRece.end(), ',', ' ');
              istringstream is(strRece);
              is >> str[0] >> str[1] >> str[2] >> str[3] >> str[4] >> str[5] >>
                  str[6] >> str[7] >> str[8] >> str[9] >> str[10] >> str[11] >>
                  str[12];
              if (str[2].compare("A") == 0) {
                GPS_data.header.stamp = ros::Time::now(); 
                GPS_data.lat = atof(str[3].c_str()) / 100.0;
                GPS_data.N_S = str[4].c_str();
                GPS_data.lon = atof(str[5].c_str()) / 100.0;
                GPS_data.E_W = str[6].c_str();
                GPS_data.groundSpeed = atof(str[7].c_str());
                GPS_data.speedDir = atof(str[8].c_str());
                GPS_data.gpsAngle = atof(str[10].c_str());
                GPS_data.gpsAngleDir = str[11].c_str();
                GPS_pub.publish(GPS_data);
              }
            }else{
              pos= strRece.find("GGA");
              if (pos != -1) {
                 if (strRece.size()<30) {
                    GPS_data.header.stamp = ros::Time::now(); 
                    GPS_data.lat = 0.0;
                    GPS_data.N_S = "";
                    GPS_data.lon = 0.0;
                    GPS_data.E_W = "";
                    GPS_data.groundSpeed = 0.0;
                    GPS_data.speedDir = 0.0;
                    GPS_data.gpsAngle = 0.0;
                    GPS_data.gpsAngleDir = "";
                    GPS_pub.publish(GPS_data);
                 }
              }
            }
            if (i + 5 < strRece.length()) {
              strRece = strRece.substr(end + 2);
            } else {
              strRece = "";
            }
            break;
          }
        }
      }
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}
